/***************************************************************************************
 The copyright in this software is being made available under the BSD License, included
 below. This software may be subject to other third party rights,including patent rights,
 and no such rights are granted under this license.

 Regents of the Tsinghua University, Peking University, Zhejiang University, HKUST
 Tsinghua University, Peking University, Zhejiang University, HKUST 2011

 Copyright (c) 2011, Regents of the Tsinghua University, Peking University, Zhejiang
 University, HKUST. All Rights Reserved.

 Redistribution and use in source and binary forms, with or without modification, are
 permitted provided that the following conditions are met:
   * Redistributions of source code must retain the above copyright notice,
     this list of conditions and the following disclaimer.
   * Redistributions in binary form must reproduce the above copyright notice,
     this list of conditions and the following disclaimer in the documentation
     and/or other materials provided with the distribution.
   * Neither the names of the copyright holders nor the names of its
     contributors may be used to endorse or promote products derived from this
     software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
 WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
 OF SUCH DAMAGE.
***************************************************************************************/

/*!
************************************************************************
*
* \file fast_me.c
*
* \brief
*   Fast integer pel motion estimation and fractional pel motion estimation
*   algorithms are described in this file.
*   1. get_mem_FME() and free_mem_FME() are functions for allocation and release
*      of memories about motion estimation
*   2. FME_BlockMotionSearch() is the function for fast integer pel motion
*      estimation and fractional pel motion estimation
*   3. DefineThreshold() defined thresholds for early termination
* \author
*    Main contributors: (see contributors.h for copyright, address and affiliation details)
*    - Zhibo Chen         <chenzhibo@tsinghua.org.cn>
*    - JianFeng Xu        <fenax@video.mdc.tsinghua.edu.cn>
*    - Wenfang Fu         <fwf@video.mdc.tsinghua.edu.cn>
*    - Xiaozhong Xu       <xxz@video.mdc.tsinghua.edu.cn>
* \date
*    2006.1
************************************************************************
*/

#include <math.h>
#include <stdlib.h>
#include <assert.h>
#include <limits.h>
#include <memory.h>

#include "contributors.h"
#include "memalloc.h"
#include "block.h"
#include "fast_me.h"

#define Q_BITS          15
#define MIN_IMG_WIDTH  176
extern  int*   byte_abs;
extern  int*   mvbits;
extern  int*   spiral_search_x;
extern  int*   spiral_search_y;

static pel_t **  ref_pic;

static const int Diamond_x[4] = {-1, 0, 1, 0};
static const int Diamond_y[4] = {0, 1, 0, -1};
static const int Hexagon_x[6] = {2, 1, -1, -2, -1, 1};
static const int Hexagon_y[6] = {0, -2, -2, 0,  2, 2};
static const int Big_Hexagon_x[16] = {0,-2, -4,-4,-4, -4, -4, -2,  0,  2,  4,  4, 4, 4, 4, 2};
static const int Big_Hexagon_y[16] = {4, 3, 2,  1, 0, -1, -2, -3, -4, -3, -2, -1, 0, 1, 2, 3};
static const int Quater_x[8] = {2, 1, 0, -1, -2, -1,  0,  1};
static const int Quater_y[8] = {0, 1, 2,  1,  0, -1, -2, -1};

// for bipred mode
static int height;
static int pred_MV_ref_flag;
//int bipred_flag;                            //flag for bipred

static const int   Multi_Ref_Thd[6]   = {0,  300,  60,  15,  120,  120};  // 16x16, 8x8, 4x4, 16x8, 8x16
static const int   Big_Hexagon_Thd[6] = {0, 3000, 800, 200, 1500, 1500};
static const int   Median_Pred_Thd[6] = {0,  750, 170,  40,  350,  350};
static const int   Threshold_DSR[6]   = {0, 2200, 500, 120, 1000, 1000};


static int Median_Pred_Thd_MB[6];
static int Big_Hexagon_Thd_MB[6];
static int Multi_Ref_Thd_MB[6];

static const int Q_TAB[64] =
{
    32768, 29775, 27554, 25268, 23170, 21247, 19369, 17770,
    16302, 15024, 13777, 12634, 11626, 10624, 9742, 8958,
    8192, 7512, 6889, 6305, 5793, 5303, 4878, 4467,
    4091, 3756, 3444, 3161, 2894, 2654, 2435, 2235,
    2048, 1878, 1722, 1579, 1449, 1329, 1218, 1117,
    1024, 939, 861, 790, 724, 664, 609, 558,
    512, 470, 430, 395, 362, 332, 304, 279,
    256, 235, 215, 197, 181, 166, 152, 140
};

int get_bid_cost( int org_start_x, int org_start_y, byte** bi_ref[2], int ref_start_x[2], int ref_start_y[2], int x_size, int y_size );

void DefineThreshold()
{
    AlphaFourth_1[1] = 0.01f;
    AlphaFourth_1[4] = 0.01f;
    AlphaFourth_1[5] = 0.01f;
    AlphaFourth_1[2] = 0.02f;
    AlphaFourth_1[3] = 0.04f;

    AlphaFourth_2[1] = 0.06f;
    AlphaFourth_2[4] = 0.07f;
    AlphaFourth_2[5] = 0.07f;
    AlphaFourth_2[2] = 0.08f;
    AlphaFourth_2[3] = 0.15f;

    DefineThresholdMB();
    return;
}

void DefineThresholdMB()
{
    int gb_q_bits    = Q_BITS;
    int Thresh4x4;

    float Quantize_step;
    int i;
    // scale factor: defined for different image sizes
    float scale_factor = ( float )( ( 1-input->FMEScale*0.1 )+input->FMEScale*0.1*( img->width/MIN_IMG_WIDTH ) );
    // QP factor: defined for different quantization steps
    float QP_factor = ( float )( ( 1.0-0.90*( input->qp_p/63.0f ) ) );

    Thresh4x4 =   ( ( 1<<gb_q_bits ) ) / Q_TAB[input->qp_p];
    Quantize_step = Thresh4x4/( 4*5.61f )*2.0f*scale_factor;

    Bsize[3]=( 16*16 )*Quantize_step;
    Bsize[2]=Bsize[3]*16;
    Bsize[4]=Bsize[2]*4;
    Bsize[5]=Bsize[2]*4;
    Bsize[1]=Bsize[2]*16;

    for( i=1; i<6; i++ )
    {
        //ET_Thd1: early termination after median prediction
        Median_Pred_Thd_MB[i]  = ( int ) ( Median_Pred_Thd[i]* scale_factor*QP_factor );
        //ET_thd2: early termination after every circle of 16 points Big-Hex Search
        Big_Hexagon_Thd_MB[i]  = ( int ) ( Big_Hexagon_Thd[i]* scale_factor*QP_factor );
        //threshold for multi ref case
        Multi_Ref_Thd_MB[i]    = ( int ) ( Multi_Ref_Thd[i]  * scale_factor*QP_factor );
        //threshold for usage of DSR technique. DSR ref to JVT-R088
        Threshold_DSR_MB[i]    = ( int ) ( Threshold_DSR[i]  * scale_factor*QP_factor );
    }
}


int get_mem_FME()
{
    int memory_size = 0;
    if ( NULL==( flag_intra = ( byte* )calloc ( ( img->width>>4 )+1,sizeof( byte ) ) ) ) no_mem_exit( "get_mem_FME: flag_intra" ); //fwf 20050330

    memory_size += get_mem2D_byte( &McostState, 2*input->search_range+1, 2*input->search_range+1 );
    memory_size += get_mem4D_int( &( fastme_ref_cost ), input->max_num_references, 6, 4, 4 );
    memory_size += get_mem3D_int( &( fastme_l0_cost ), 6, img->height/8, img->width/8 );
    memory_size += get_mem3D_int( &( fastme_l1_cost ), 6, img->height/8, img->width/8 );

    if( input->bi_me == 1 ) //memory allocation for bipred mode
    {
        memory_size += get_mem3D_int( &( fastme_l0_cost_bipred ), 6, img->height/8, img->width/8 ); //for bipred
        memory_size += get_mem3D_int( &( fastme_l1_cost_bipred ), 6, img->height/8, img->width/8 ); //for bipred
    }
    return memory_size;
}


void free_mem_FME()
{
    free_mem2D_all( ( void** )McostState );
    free_mem4D_int( fastme_ref_cost, input->max_num_references, 6 );
    free_mem3D_int( fastme_l0_cost, 6 );
    free_mem3D_int( fastme_l1_cost, 6 );
    free ( flag_intra );
    if( input->bi_me == 1 )
    {
        free_mem3D_int( fastme_l0_cost_bipred, 6 ); //for bipred
        free_mem3D_int( fastme_l1_cost_bipred, 6 ); //for bipred
    }
}


int PartCalMadFME( byte** org, int org_start_x, int org_start_y, byte** ref, int ref_start_x, int ref_start_y, int x_size, int y_size, int min_mcost )
{
    int y, x;
    int mcost = 0;
    int blocksize_x4 = x_size >> 2;
    pel_t *src, *dst;

    if ( ref_start_x > img->p_width-x_size -  IMG_PAD_SIZE || ref_start_y > img->p_height-y_size -  IMG_PAD_SIZE || ref_start_x < -IMG_PAD_SIZE || ref_start_y < - IMG_PAD_SIZE )
    {
        return ( MAX_VALUE>>1 );
    }

    ref_start_y += IMG_PAD_SIZE;
    ref_start_x += IMG_PAD_SIZE;

    for ( y = 0; y < y_size; y++ )
    {
        src = &org[org_start_y + y][org_start_x];
        dst = &ref[ref_start_y + y][ref_start_x];
        for ( x = 0; x < blocksize_x4; x++ )
        {
            mcost += byte_abs[ *src++ - *dst++ ];
            mcost += byte_abs[ *src++ - *dst++ ];
            mcost += byte_abs[ *src++ - *dst++ ];
            mcost += byte_abs[ *src++ - *dst++ ];
        }

        if ( mcost >= min_mcost )
        {
            break;
        }
    }
    return mcost;
}


int PartCalMadFME_bid( int mb_pix_x, int mb_pix_y, byte ***bi_ref, int *target_x, int *target_y, int blocksize_x, int blocksize_y, int refine_ref_idx )
{
    //  if (target_x[refine_ref_idx] > img->p_width-IMG_PAD_SIZE-blocksize_x || target_y[refine_ref_idx] > img->p_height-IMG_PAD_SIZE-blocksize_y ||
    //    target_x[refine_ref_idx] < -IMG_PAD_SIZE || target_y[refine_ref_idx] < -IMG_PAD_SIZE)
    //Dai Wei 110924
    if ( target_x[refine_ref_idx] > img->p_width-IMG_PAD_SIZE-blocksize_x || target_y[refine_ref_idx] > img->p_height-IMG_PAD_SIZE-blocksize_y || target_x[refine_ref_idx] < -IMG_PAD_SIZE || target_y[refine_ref_idx] < -IMG_PAD_SIZE || target_x[1 - refine_ref_idx] > img->p_width-IMG_PAD_SIZE-blocksize_x || target_y[1 - refine_ref_idx] > img->p_height-IMG_PAD_SIZE-blocksize_y || target_x[1 - refine_ref_idx] < -IMG_PAD_SIZE || target_y[1 - refine_ref_idx] < -IMG_PAD_SIZE )
    {
        return ( MAX_VALUE>>1 );
    }
    else
        return get_bid_cost( mb_pix_x, mb_pix_y, bi_ref, target_x, target_y, blocksize_x, blocksize_y );
}



/*!
************************************************************************
* \brief
*    FastIntegerPelBlockMotionSearch: fast pixel block motion search
*    this algorithm is called UMHexagonS, which includes four steps
*    with different kinds of search patterns
************************************************************************
*/
int                                     //  ==> minimum motion cost after search
FastIntegerPelBlockMotionSearch  ( short     ref,         // <--  reference frame (0... or -1 (backward))
                                   int       list,
                                   int       pic_pix_x,    // <--  absolute x-coordinate of regarded AxB block
                                   int       pic_pix_y,    // <--  absolute y-coordinate of regarded AxB block
                                   int       blocktype,    // <--  block type (1-16x16 ... 7-4x4)
                                   mv_t*     pred_mv,
                                   mv_t*     mv,
                                   int       search_range ) // <--  1-d search range in pel units

{
    int    pos, cand_x, cand_y,  mcost;
#if QUARTER_PIXEL
    int    mvshift       = 2;
#else
    int    mvshift       = 1;                  // motion vector shift for getting sub-pel units
#endif
    int    blocksize_y   = input->blk_size[blocktype][1];            // vertical block size
    int    blocksize_x   = input->blk_size[blocktype][0];            // horizontal block size
#if QUARTER_PIXEL
    int    pred_x        = pic_pix_x + ROUNDING( pred_mv->x );     // predicted position x (in pel units)
    int    pred_y        = pic_pix_y + ROUNDING( pred_mv->y );     // predicted position y (in pel units)
#else
    int    pred_x        = pic_pix_x + ( pred_mv->x >> mvshift );     // predicted position x (in pel units)
    int    pred_y        = pic_pix_y + ( pred_mv->y >> mvshift );     // predicted position y (in pel units)
#endif
    int    center_x      = pic_pix_x + mv->x;                        // center position x (in pel units)
    int    center_y      = pic_pix_y + mv->y;                        // center position y (in pel units)
    int    best_x = 0, best_y = 0;
    int    search_step,iYMinNow, iXMinNow;
    int    i,m,j;
    float  betaFourth_1,betaFourth_2;
    int     temp_Big_Hexagon_x[16];//  temp for Big_Hexagon_x;
    int     temp_Big_Hexagon_y[16];//  temp for Big_Hexagon_y;
    int    mb_pix_x = pic_pix_x - img->pix_x;
    int    mb_pix_y = pic_pix_y - img->pix_y;
    int    min_mcost = MAX_VALUE;

    short  block_x = ( mb_pix_x >> 3 );
    short  block_y = ( mb_pix_y >> 3 );
    int    lambda_factor = img->lambda_motion_factor;
    int    ET_Thred = Median_Pred_Thd_MB[blocktype];//ET threshold in use
    int    sad_pred;
    MVPInfo mvpInfo;

    mvpInfo.mvCand[0].x   = center_x;
    mvpInfo.mvCand[0].y   = center_y;
    mvpInfo.mvCand[0].ref = ref;
    mvpInfo.iNum = 1;

    mvpInfo.mvCand[1].x   = 0;
    mvpInfo.mvCand[1].y   = 0;
    mvpInfo.mvCand[1].ref = ref;
    mvpInfo.iNum ++;

    height = img->height;
    ref_pic = img_y_rec_space[ref];

    //////allocate memory for search state//////////////////////////
    memset( McostState[0],0,( 2*input->search_range+1 )*( 2*input->search_range+1 ) );

    //check the center median predictor
    cand_x = center_x ;
    cand_y = center_y ;
    mcost = MV_COST ( lambda_factor, mvshift, cand_x, cand_y, pred_x, pred_y );
    mcost += PartCalMadFME( img->org_y_data, mb_pix_x, mb_pix_y, ref_pic, cand_x, cand_y, blocksize_x, blocksize_y, min_mcost );
    McostState[search_range][search_range] = 1;

    if ( mcost < min_mcost )
    {
        min_mcost = mcost;
        best_x = cand_x;
        best_y = cand_y;
    }

    iXMinNow = best_x;
    iYMinNow = best_y;
    for ( m = 0; m < 4; m++ )
    {
        cand_x = iXMinNow + Diamond_x[m];
        cand_y = iYMinNow + Diamond_y[m];
        SEARCH_ONE_PIXEL
    }

    if( center_x != pic_pix_x || center_y != pic_pix_y )
    {
        cand_x = pic_pix_x ;
        cand_y = pic_pix_y ;
        SEARCH_ONE_PIXEL

        iXMinNow = best_x;
        iYMinNow = best_y;
        for ( m = 0; m < 4; m++ )
        {
            cand_x = iXMinNow + Diamond_x[m];
            cand_y = iYMinNow + Diamond_y[m];
            SEARCH_ONE_PIXEL
        }
    }

    //(MV and SAD prediction)
    sad_pred = deriveMVPandSADFME ( &mvpInfo, ref, mb_pix_x, mb_pix_y, blocktype, list );
    setup_FME( ref, list, block_y, block_x, blocktype );

    if ( sad_pred > 0 )
        ET_Thred = RFM_MIN( sad_pred , ET_Thred );

    // Threshold defined for EARLY_TERMINATION
    if ( pred_SAD == 0 )
    {
        betaFourth_1=0;
        betaFourth_2=0;
    }
    else
    {
        betaFourth_1 = Bsize[blocktype]/( pred_SAD*pred_SAD )-AlphaFourth_1[blocktype];
        betaFourth_2 = Bsize[blocktype]/( pred_SAD*pred_SAD )-AlphaFourth_2[blocktype];
    }

    // first_step: initial start point prediction
    if( blocktype>1 )
    {
        cand_x = pic_pix_x + ( pred_MV_uplayer[0]>>mvshift );
        cand_y = pic_pix_y + ( pred_MV_uplayer[1]>>mvshift );
        SEARCH_ONE_PIXEL
    }

    //prediction using mV of last ref motion vector
    if( pred_MV_ref_flag == 1 )        //Notes: for interlace case, ref==1 should be added
    {
        cand_x = pic_pix_x + ( pred_MV_ref[0]>>mvshift );
        cand_y = pic_pix_y + ( pred_MV_ref[1]>>mvshift );
        SEARCH_ONE_PIXEL
    }

    for ( i = 1; i < mvpInfo.iNum; i++ )
    {
        cand_x = pic_pix_x + ( mvpInfo.mvCand[i].x >> mvshift );
        cand_y = pic_pix_y + ( mvpInfo.mvCand[i].y >> mvshift );
        SEARCH_ONE_PIXEL
    }

    // Reposition here
    if( !input->NoET && min_mcost < ET_Thred )
    {
        goto terminate_step;
    }
    ET_Thred = Big_Hexagon_Thd_MB[blocktype];  // ET_Thd2: early termination Threshold for strong motion

    if ( sad_pred > 0 )
        ET_Thred = RFM_MIN( sad_pred , ET_Thred );

    //small local search
    iXMinNow = best_x;
    iYMinNow = best_y;
    for ( m = 0; m < 4; m++ )
    {
        cand_x = iXMinNow + Diamond_x[m];
        cand_y = iYMinNow + Diamond_y[m];
        SEARCH_ONE_PIXEL
    }

    //early termination algorithm, refer to JVT-G016
    EARLY_TERMINATION

    if( blocktype>4 )
        goto fourth_1_step;
    else
        goto sec_step;

sec_step: //Unsymmetrical-cross search
    iXMinNow = best_x;
    iYMinNow = best_y;

    for( i = 1; i < search_range; i+=2 )
    {
        search_step = i;
        cand_x = iXMinNow + search_step;
        cand_y = iYMinNow ;
        SEARCH_ONE_PIXEL

        cand_x = iXMinNow - search_step;
        cand_y = iYMinNow;
        SEARCH_ONE_PIXEL
    }
    for( i = 1; i < ( search_range/2 ); i+=2 )
    {
        search_step = i;
        cand_x = iXMinNow ;
        cand_y = iYMinNow + search_step;
        SEARCH_ONE_PIXEL
        cand_x = iXMinNow ;
        cand_y = iYMinNow - search_step;
        SEARCH_ONE_PIXEL
    }

    //early termination alogrithm, refer to JVT-G016
    EARLY_TERMINATION

    iXMinNow = best_x;
    iYMinNow = best_y;

    //third_step:    // Uneven Multi-Hexagon-grid Search
    //sub step 1: 5x5 squre search
    for( pos=1; pos<25; pos++ )
    {
        cand_x = iXMinNow + spiral_search_x[pos];
        cand_y = iYMinNow + spiral_search_y[pos];
        SEARCH_ONE_PIXEL
    }

    //early termination alogrithm, refer to JVT-G016
    EARLY_TERMINATION

    //sub step 2:  Multi-Hexagon-grid search
    memcpy( temp_Big_Hexagon_x,Big_Hexagon_x,64 );
    memcpy( temp_Big_Hexagon_y,Big_Hexagon_y,64 );
    for( i=1; i<=( search_range/4 ); i++ )
    {
        for ( m = 0; m < 16; m++ )
        {
            cand_x = iXMinNow + temp_Big_Hexagon_x[m];
            cand_y = iYMinNow + temp_Big_Hexagon_y[m];
            temp_Big_Hexagon_x[m] += Big_Hexagon_x[m];
            temp_Big_Hexagon_y[m] += Big_Hexagon_y[m];
            SEARCH_ONE_PIXEL
        }
        // ET_Thd2: early termination Threshold for strong motion
        if( !input->NoET && min_mcost < ET_Thred )
        {
            goto terminate_step;
        }
    }

    //fourth_step:  //Extended Hexagon-based Search
    // the fourth step with a small search pattern
fourth_1_step:  //sub step 1: small Hexagon search
    for( i=0; i < search_range; i++ ) //change into 1/2
    {
        iXMinNow = best_x;
        iYMinNow = best_y;
        for ( m = 0; m < 6; m++ )
        {
            cand_x = iXMinNow + Hexagon_x[m];
            cand_y = iYMinNow + Hexagon_y[m];
            SEARCH_ONE_PIXEL
        }
        if( best_x == iXMinNow && best_y == iYMinNow )
            break;
    }
fourth_2_step: //sub step 2: small Diamond search
    for( i = 0; i < search_range; i++ ) //change into 1/2
    {
        iXMinNow = best_x;
        iYMinNow = best_y;
        for ( m = 0; m < 4; m++ )
        {
            cand_x = iXMinNow + Diamond_x[m];
            cand_y = iYMinNow + Diamond_y[m];
            SEARCH_ONE_PIXEL
        }
        if( best_x == iXMinNow && best_y == iYMinNow )
            break;
    }
terminate_step:
    // store SAD information for prediction
    //FAST MOTION ESTIMATION. ZHIBO CHEN 2003.3
    for ( i=0; i < ( blocksize_x>>3 ); i++ )
    {
        for ( j=0; j < ( blocksize_y>>3 ); j++ )
        {
            if( list == 0 )
            {
                fastme_ref_cost[ref][blocktype][block_y + j][block_x + i] = min_mcost;
                fastme_l0_cost[blocktype][( img->pix_y>>3 ) + block_y + j][( img->pix_x>>3 ) + block_x + i] = min_mcost;
            }
            else
            {
                fastme_l1_cost[blocktype][( img->pix_y>>3 ) + block_y + j][( img->pix_x>>3 ) + block_x + i] = min_mcost;
            }
        }
    }

    mv->x = best_x - pic_pix_x;
    mv->y = best_y - pic_pix_y;

#if QUARTER_PIXEL
    mv->x <<= 2;
    mv->y <<= 2;
#else
    mv->x <<= 1;
    mv->y <<= 1;
#endif
    return min_mcost;
}


/*!
************************************************************************
* \brief
*    FastBipredIntegerPelBlockMotionSearch: fast pixel block motion search
*    this algorithm is called UMHexagonS, which includes four steps
*    with different kinds of search patterns
************************************************************************
*/
int
FastBipredIntegerPelBlockMotionSearch (  int pic_pix_y,
        int pic_pix_x,
        int blocktype,
        mv_t *pred_mv,
        mv_t *pred_fix_mv,
        mv_t* fix_mv,
        mv_t *refine_mv,
        int min_cost
                                      )

{
    int    temp_Big_Hexagon_x[16];// = Big_Hexagon_x;
    int    temp_Big_Hexagon_y[16];// = Big_Hexagon_y;
#if QUARTER_PIXEL
    int   mvshift       = 2;                  // motion vector shift for getting sub-pel units
#else
    int   mvshift       = 1;                  // motion vector shift for getting sub-pel units
#endif
    int   search_step,iYMinNow, iXMinNow;
    int   i,m,j;
    float betaFourth_1,betaFourth_2;
    int   cand_x, cand_y, mcost;
    int   target_x[2], target_y[2], refine_ref_idx, fix_ref_idx;
    int   search_range = input->search_range;
    int   blocksize_y   = input->blk_size[blocktype][1];            // vertical block size
    int   blocksize_x   = input->blk_size[blocktype][0];            // horizontal block size

#if QUARTER_PIXEL
    int    pred_x        = pic_pix_x + ROUNDING( pred_mv->x );
    int    pred_y        = pic_pix_y + ROUNDING( pred_mv->y );
#else
    int   pred_x       = pic_pix_x + ( pred_mv->x >> mvshift );
    int   pred_y       = pic_pix_y + ( pred_mv->y >> mvshift );
#endif
    int   s_center_x   = pic_pix_x + refine_mv->x;  //search center
    int   s_center_y   = pic_pix_y + refine_mv->y;
    int   mb_pix_x     = pic_pix_x - img->pix_x;
    int   mb_pix_y     = pic_pix_y - img->pix_y;
    byte **bi_ref[2];

    int   min_mcost = MAX_VALUE;
    int   best_x = s_center_x;
    int   best_y = s_center_y;
    int   ET_Thred = Median_Pred_Thd_MB[blocktype];
    int   sad_pred;
    MVPInfo mvpInfo;

    refine_ref_idx = pred_mv->ref;
    fix_ref_idx    = 1 - refine_ref_idx;
    bi_ref[refine_ref_idx] = img_y_rec_space[refine_ref_idx];
    bi_ref[fix_ref_idx]    = img_y_rec_space[fix_ref_idx];
    target_x[fix_ref_idx]  = pic_pix_x + fix_mv->x;
    target_y[fix_ref_idx]  = pic_pix_y + fix_mv->y;

    mvpInfo.mvCand[0].x   = s_center_x;
    mvpInfo.mvCand[0].y   = s_center_y;
    mvpInfo.mvCand[0].ref = refine_ref_idx;
    mvpInfo.iNum = 1;

    mvpInfo.mvCand[1].x   = 0;
    mvpInfo.mvCand[1].y   = 0;
    mvpInfo.mvCand[1].ref = refine_ref_idx;
    mvpInfo.iNum ++;

    //////allocate memory for search state//////////////////////////
    memset( McostState[0],0,( 2*search_range+1 )*( 2*search_range+1 ) );

    //check the center median predictor
    cand_x = s_center_x ;
    cand_y = s_center_y ;
    target_x[refine_ref_idx] = cand_x;
    target_y[refine_ref_idx] = cand_y;
    mcost   = MV_COST( img->lambda_motion_factor, mvshift, target_x[refine_ref_idx], target_y[refine_ref_idx], pred_x, pred_y );
    mcost  += MV_COST( img->lambda_motion_factor, mvshift, fix_mv->x,    fix_mv->y, pred_fix_mv->x >> mvshift, pred_fix_mv->y >> mvshift );
    mcost  += PartCalMadFME_bid( mb_pix_x, mb_pix_y, bi_ref, target_x, target_y, blocksize_x, blocksize_y, refine_ref_idx );
    McostState[search_range][search_range] = 1;
    if ( mcost < min_mcost )
    {
        min_mcost = mcost;
        best_x = cand_x;
        best_y = cand_y;
    }

    iXMinNow = best_x;
    iYMinNow = best_y;
    for ( m = 0; m < 4; m++ )
    {
        cand_x = iXMinNow + Diamond_x[m];
        cand_y = iYMinNow + Diamond_y[m];
        SEARCH_ONE_PIXEL_BIPRED
    }

    if( s_center_x != pic_pix_x || s_center_y != pic_pix_y )
    {
        cand_x = pic_pix_x ;
        cand_y = pic_pix_y ;
        SEARCH_ONE_PIXEL_BIPRED

        iXMinNow = best_x;
        iYMinNow = best_y;
        for ( m = 0; m < 4; m++ )
        {
            cand_x = iXMinNow + Diamond_x[m];
            cand_y = iYMinNow + Diamond_y[m];
            SEARCH_ONE_PIXEL_BIPRED
        }
    }

    sad_pred = deriveMVPandSADFME ( &mvpInfo, refine_ref_idx, mb_pix_x, mb_pix_y, blocktype, refine_ref_idx );
    if ( sad_pred > 0 )
        ET_Thred = RFM_MIN( sad_pred , ET_Thred );
    /***********************************init process*************************/

    //if( min_mcost < ET_Thred)
    //{
    //  goto terminate_step;
    //}
    //else
    {
        int  N_Bframe=0;
        int  n_Bframe=0;
        mv_t mv_ref = img->mv[refine_ref_idx][blocktype][mb_pix_y >> 3][mb_pix_x >> 3];
        N_Bframe = input->succe_b;
        n_Bframe = tot_b_frm_num % ( N_Bframe+1 );

        /**************************** MV prediction **********************/
        //MV uplayer prediction
        // non for bipred mode
        //MV ref-frame prediction

        if( refine_ref_idx == 0 )
        {
            pred_MV_ref[0] =( int ) ( mv_ref.ref*( -n_Bframe )/( N_Bframe-n_Bframe+1.0f ) );
            pred_MV_ref[1] =( int ) ( mv_ref.ref*( -n_Bframe )/( N_Bframe-n_Bframe+1.0f ) );
        }
        /******************************SAD prediction**********************************/
        pred_SAD = RFM_MIN( RFM_MIN( SAD_a, SAD_b ), SAD_c ); // pred_SAD_space

        ///////Threshold defined for early termination///////////////////
        if ( pred_SAD == 0 )
        {
            betaFourth_1=0;
            betaFourth_2=0;
        }
        else
        {
            betaFourth_1 = Bsize[blocktype]/( pred_SAD*pred_SAD )-AlphaFourth_1[blocktype];
            betaFourth_2 = Bsize[blocktype]/( pred_SAD*pred_SAD )-AlphaFourth_2[blocktype];
        }
    }

    /***********************************end of init *************************/

    // first_step: initial start point prediction
    //prediction using mV of last ref motion vector
    if( refine_ref_idx == 0 )
    {
        cand_x = pic_pix_x + ( pred_MV_ref[0] >> mvshift );
        cand_y = pic_pix_y + ( pred_MV_ref[1] >> mvshift );
        SEARCH_ONE_PIXEL_BIPRED
    }

    for ( i = 1; i < mvpInfo.iNum; i++ )
    {
        cand_x = pic_pix_x + ( mvpInfo.mvCand[i].x >> mvshift );
        cand_y = pic_pix_y + ( mvpInfo.mvCand[i].y >> mvshift );
        SEARCH_ONE_PIXEL_BIPRED
    }

    // Reposition here
    if( !input->NoET && min_mcost < ET_Thred )
    {
        goto terminate_step;
    }

    ET_Thred = Big_Hexagon_Thd_MB[blocktype];
    if ( sad_pred > 0 )
        ET_Thred = RFM_MIN( sad_pred , ET_Thred );
    //small local search
    iXMinNow = best_x;
    iYMinNow = best_y;
    for ( m = 0; m < 4; m++ )
    {
        cand_x = iXMinNow + Diamond_x[m];
        cand_y = iYMinNow + Diamond_y[m];
        SEARCH_ONE_PIXEL_BIPRED
    }

    //early termination alogrithm, refer to JVT-G016
    EARLY_TERMINATION

    //sec_step: //Unsymmetrical-cross search
    iXMinNow = best_x;
    iYMinNow = best_y;

    for( i = 1; i < search_range; i+=2 )
    {
        search_step = i;
        cand_x = iXMinNow + search_step;
        cand_y = iYMinNow ;
        SEARCH_ONE_PIXEL_BIPRED
        cand_x = iXMinNow - search_step;
        cand_y = iYMinNow ;
        SEARCH_ONE_PIXEL_BIPRED
    }

    for( i = 1; i < ( search_range/2 ); i+=2 )
    {
        search_step = i;
        cand_x = iXMinNow ;
        cand_y = iYMinNow + search_step;
        SEARCH_ONE_PIXEL_BIPRED
        cand_x = iXMinNow ;
        cand_y = iYMinNow - search_step;
        SEARCH_ONE_PIXEL_BIPRED
    }
    //early termination alogrithm, refer to JVT-G016
    EARLY_TERMINATION

    //third_step:     // Uneven Multi-Hexagon-grid Search
    iXMinNow = best_x;
    iYMinNow = best_y;

    //sub step1: 5x5 square search
    for( i=1; i<25; i++ )
    {
        cand_x = iXMinNow + spiral_search_x[i];
        cand_y = iYMinNow + spiral_search_y[i];
        SEARCH_ONE_PIXEL_BIPRED
    }

    //early termination alogrithm, refer to JVT-G016
    EARLY_TERMINATION      //added back by xxz

    //sub step2: multi-grid-hexagon-search
    memcpy( temp_Big_Hexagon_x,Big_Hexagon_x,64 );
    memcpy( temp_Big_Hexagon_y,Big_Hexagon_y,64 );
    for( i=1; i<=( input->search_range>>2 ); i++ )
    {
        for ( m = 0; m < 16; m++ )
        {
            cand_x = iXMinNow + temp_Big_Hexagon_x[m];
            cand_y = iYMinNow + temp_Big_Hexagon_y[m];
            temp_Big_Hexagon_x[m] += Big_Hexagon_x[m];
            temp_Big_Hexagon_y[m] += Big_Hexagon_y[m];

            SEARCH_ONE_PIXEL_BIPRED
        }
        if( !input->NoET && min_mcost < ET_Thred )
        {
            goto terminate_step;
        }
    }
    //fourth step: Local Refinement: Extended Hexagon-based Search
fourth_1_step:
    for( i=0; i < search_range; i++ )
    {
        iXMinNow = best_x;
        iYMinNow = best_y;
        for ( m = 0; m < 6; m++ )
        {
            cand_x = iXMinNow + Hexagon_x[m];
            cand_y = iYMinNow + Hexagon_y[m];
            SEARCH_ONE_PIXEL_BIPRED
        }
        if( best_x == iXMinNow && best_y == iYMinNow )
            break;
    }
fourth_2_step:

    for( i = 0; i < search_range; i++ )
    {
        iXMinNow = best_x;
        iYMinNow = best_y;
        for ( m = 0; m < 4; m++ )
        {
            cand_x = iXMinNow + Diamond_x[m];
            cand_y = iYMinNow + Diamond_y[m];
            SEARCH_ONE_PIXEL_BIPRED
        }
        if( best_x == iXMinNow && best_y == iYMinNow )
            break;
    }

terminate_step:
    for ( i=0; i < ( blocksize_x>>3 ); i++ )
    {
        for ( j=0; j < ( blocksize_y>>3 ); j++ )
        {
            if( refine_ref_idx == 0 )
            {
                fastme_l0_cost_bipred[blocktype][( img->pix_y>>3 ) + ( mb_pix_y >> 3 ) + j][( img->pix_x>>3 ) + ( mb_pix_x >> 3 ) + i] = min_mcost;
            }
            else
            {
                fastme_l1_cost_bipred[blocktype][( img->pix_y>>3 ) + ( mb_pix_y >> 3 ) + j][( img->pix_x>>3 ) + ( mb_pix_x >> 3 ) +i] = min_mcost;
            }
        }
    }

    refine_mv->x = best_x - pic_pix_x;
    refine_mv->y = best_y - pic_pix_y;
#if QUARTER_PIXEL
    refine_mv->x <<= 2;
    refine_mv->y <<= 2;
    fix_mv->x    <<= 2;
    fix_mv->y    <<= 2;
#else
    refine_mv->x <<= 1;
    refine_mv->y <<= 1;
    fix_mv->x    <<= 1;
    fix_mv->y    <<= 1;
#endif
    return min_mcost;
}

/*!
************************************************************************
* \brief
* Functions for SAD prediction in FastME
* Use SAD_prediction to revise the current threshold
*
* Functions for MVP derivation in FastME
*   B C
* A E
* D
************************************************************************
*/
int deriveMVPandSADFME( MVPInfo* pInfo, int ref_idx, int mb_pix_x, int mb_pix_y, int mode, int list )
{
    static const int inc_sizex[6] = {15, 15, 7, 3, 15,  7};
    static const int inc_sizey[6] = {15, 15, 7, 3,  7, 15};
    NeighborData neighbor;
    mv_t         mvpC[MAX_MV_CAND];
    int          i, j, idx;
    int          sad_pred_top  = INT_MAX;
    int          sad_pred_left = INT_MAX;
    int          sad_pred;

    int ***l0_cost, ***l1_cost;

    l0_cost = bipred_flag == 0 ? fastme_l0_cost : fastme_l0_cost_bipred;
    l1_cost = bipred_flag == 0 ? fastme_l1_cost : fastme_l1_cost_bipred;

    if ( bipred_flag == 1 )
    {
        SAD_a = 0;
        SAD_b = 0;
        SAD_c = -1;
        SAD_d = 0;
    }

    assert ( pInfo->iNum >= 1 );

    if ( deriveNeighbors( &neighbor, mb_pix_x, mb_pix_y, MB_LEFT ) )
    {
        addMVPCandFME( pInfo, &neighbor, mode, ref_idx, list );
        sad_pred_left = list == 0 ? l0_cost[mode][neighbor.block_y][neighbor.block_x] : l1_cost[mode][neighbor.block_y][neighbor.block_x];
        SAD_a = sad_pred_left;
    }
    if ( deriveNeighbors( &neighbor, mb_pix_x, mb_pix_y, MB_TOP ) )
    {
        addMVPCandFME( pInfo, &neighbor, mode, ref_idx, list );
        sad_pred_top = list == 0 ? l0_cost[mode][neighbor.block_y][neighbor.block_x] : l1_cost[mode][neighbor.block_y][neighbor.block_x];
        SAD_b = sad_pred_top;
    }
    if ( deriveNeighbors( &neighbor, mb_pix_x + inc_sizex[mode], mb_pix_y, MB_ABOVE_RIGHT ) )
    {
        addMVPCandFME( pInfo, &neighbor, mode, ref_idx, list );
        SAD_c = list == 0 ? l0_cost[mode][neighbor.block_y][neighbor.block_x] : l1_cost[mode][neighbor.block_y][neighbor.block_x];
    }
    if ( deriveNeighbors( &neighbor, mb_pix_x, mb_pix_y + inc_sizey[mode], MB_BELOW_LEFT ) )
    {
        addMVPCandFME( pInfo, &neighbor, mode, ref_idx, list );
        SAD_d = list == 0 ? l0_cost[mode][neighbor.block_y][neighbor.block_x] : l1_cost[mode][neighbor.block_y][neighbor.block_x];
    }

    if ( bipred_flag == 1 )
    {
        SAD_c = ( SAD_c == -1 ? SAD_d : SAD_c );
    }

    addMVPCandTempFME( pInfo, mb_pix_x, mb_pix_y, ref_idx, mode, list );

    sad_pred = RFM_MIN( sad_pred_left, sad_pred_top );

    if ( pInfo->iNum == 1 )
    {
        return ( sad_pred == INT_MAX ?  -1 : sad_pred );
    }

    idx = 0;
    mvpC[idx++] = pInfo->mvCand[0];
    for ( i = 1; i < pInfo->iNum; i++ )
    {
        for ( j = 0; j < idx; j++ )
        {
            if ( isEqualMV( &( pInfo->mvCand[i] ), &( mvpC[j] ) ) )
                break;
        }
        if ( j == idx )
            mvpC[idx++] = pInfo->mvCand[i];
    }

    for ( i=0; i<idx; i++ )
        pInfo->mvCand[i] = mvpC[i];
    pInfo->iNum = idx;

    return ( sad_pred == INT_MAX ?  -1 : sad_pred );
}


int isEqualMV( mv_t *pmv0, mv_t *pmv1 )
{
    if ( pmv0->ref == pmv1->ref && pmv0->x == pmv1->x && pmv0->y == pmv1->y )
        return 1;
    return 0;
}

void addMVPCandFME( MVPInfo* pInfo, NeighborData *p_Neighbor, int mode, int ref_idx, int list )
{
    mv_t   mvBi[2], mvCand;
    static mv_t zero_mv   = {0, 0, 0};
    static mv_t zero_mv_b = {0, 0, 1};
    macroblock_t *currMB = &img->mb_data[img->curr_mb_nr];
    int is_currentMB = p_Neighbor->p_MB == currMB ? 1 : 0;

    int neighbor_mbtype = p_Neighbor->p_MB->mb_type;

    if ( img->type == P_IMG ) //P frame
    {
        if ( !is_currentMB )
        {
            if ( neighbor_mbtype != INTER8x8 )
            {
                assert ( neighbor_mbtype < INTER8x8 );
                mvCand = frm_mv[MAX_REF_NUM][p_Neighbor->block_y][p_Neighbor->block_x];
            }
            else
            {
                assert ( p_Neighbor->mb_block_y < 2 && p_Neighbor->mb_block_x < 2 );
                switch ( p_Neighbor->p_MB->subblock_type[ p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x ] )
                {
                    case SUBBLOCK_INTER4x4:
                        mvCand = frm_mv_4x4[MAX_REF_NUM][p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4];
                        break;
                    case SUBBLOCK_INTER8x8:
                        mvCand = frm_mv[MAX_REF_NUM][p_Neighbor->block_y][p_Neighbor->block_x];
                        break;
                    default:
                        mvCand = zero_mv;
                        break;
                }
            }
        }
        else
        {
            if ( mode == INTER8x8 )
            {
                //assert( p_Neighbor->mb_block_x == 0 );
                if ( currMB->subblock_type[ p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x ] == SUBBLOCK_INTER8x8 )
                    mvCand = frm_mv[MAX_REF_NUM][p_Neighbor->block_y][p_Neighbor->block_x];
                else if ( currMB->subblock_type[ p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x ] == SUBBLOCK_INTER4x4 )
                    mvCand = frm_mv_4x4[MAX_REF_NUM][p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4];
                else
                    mvCand = zero_mv;
            }
            else if( mode == INTER4x4 )
            {
                if ( p_Neighbor->mb_block_x == p_Neighbor->curr_mb_block_x && p_Neighbor->mb_block_y == p_Neighbor->curr_mb_block_y )
                {
                    mvCand = frm_mv_4x4[MAX_REF_NUM][p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4];
                }
                else
                {
                    if ( currMB->subblock_type[p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x] == SUBBLOCK_INTER8x8 )
                        mvCand = frm_mv[MAX_REF_NUM][p_Neighbor->block_y][p_Neighbor->block_x];
                    else if ( currMB->subblock_type[p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x] == SUBBLOCK_INTER4x4 )
                        mvCand = frm_mv_4x4[MAX_REF_NUM][p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4];
                    else
                        mvCand = zero_mv;
                }
            }
            else
                assert( 0 );
        }

        assert( mvCand.ref != -1 );
        pInfo->mvCand[ pInfo->iNum++ ] = mvCand;
    }
    else  // B_IMG
    {
        if ( !is_currentMB )
        {
            if ( neighbor_mbtype != INTER8x8 )
            {
                assert ( neighbor_mbtype < INTER8x8 );
                mvBi[0] = frm_bi_mv[p_Neighbor->block_y][p_Neighbor->block_x].fw_mv;
                mvBi[1] = frm_bi_mv[p_Neighbor->block_y][p_Neighbor->block_x].bw_mv;
                //mvCand = frm_mv[MAX_REF_NUM][p_Neighbor->block_y][p_Neighbor->block_x];
            }
            else
            {
                assert ( p_Neighbor->mb_block_y < 2 && p_Neighbor->mb_block_x < 2 );
                switch ( p_Neighbor->p_MB->subblock_type[ p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x ] )
                {
                    case SUBBLOCK_INTER4x4:
                        mvBi[0] = frm_bi_mv_4x4[p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4].fw_mv;
                        mvBi[1] = frm_bi_mv_4x4[p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4].bw_mv;
                        break;
                    case SUBBLOCK_INTER8x8:
                        mvBi[0] = frm_bi_mv[p_Neighbor->block_y][p_Neighbor->block_x].fw_mv;
                        mvBi[1] = frm_bi_mv[p_Neighbor->block_y][p_Neighbor->block_x].bw_mv;
                        break;
                    default:
                        mvBi[0] = zero_mv;
                        mvBi[1] = zero_mv_b;
                        break;
                }
            }
        }
        else
        {
            if ( mode == INTER8x8 )
            {
                if ( currMB->subblock_type[ p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x ] == SUBBLOCK_INTER8x8 )
                {
                    mvBi[0] = frm_bi_mv[p_Neighbor->block_y][p_Neighbor->block_x].fw_mv;
                    mvBi[1] = frm_bi_mv[p_Neighbor->block_y][p_Neighbor->block_x].bw_mv;
                }
                else if ( currMB->subblock_type[ p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x ] == SUBBLOCK_INTER4x4 )
                {
                    mvBi[0] = frm_bi_mv_4x4[p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4].fw_mv;
                    mvBi[1] = frm_bi_mv_4x4[p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4].bw_mv;
                }
                else
                {
                    mvBi[0] = zero_mv;
                    mvBi[1] = zero_mv_b;
                }

            }
            else if ( mode == INTER4x4 )
            {
                if ( p_Neighbor->mb_block_x == p_Neighbor->curr_mb_block_x && p_Neighbor->mb_block_y == p_Neighbor->curr_mb_block_y )
                {
                    mvBi[0] = frm_bi_mv_4x4[p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4].fw_mv;
                    mvBi[1] = frm_bi_mv_4x4[p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4].bw_mv;
                }
                else
                {
                    if ( currMB->subblock_type[ p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x ] == SUBBLOCK_INTER8x8 )
                    {
                        mvBi[0] = frm_bi_mv[p_Neighbor->block_y][p_Neighbor->block_x].fw_mv;
                        mvBi[1] = frm_bi_mv[p_Neighbor->block_y][p_Neighbor->block_x].bw_mv;
                    }
                    else if ( currMB->subblock_type[ p_Neighbor->mb_block_y*2 + p_Neighbor->mb_block_x ] == SUBBLOCK_INTER4x4 )
                    {
                        mvBi[0] = frm_bi_mv_4x4[p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4].fw_mv;
                        mvBi[1] = frm_bi_mv_4x4[p_Neighbor->block_y][p_Neighbor->block_x][p_Neighbor->block_y4][p_Neighbor->block_x4].bw_mv;
                    }
                    else
                    {
                        mvBi[0] = zero_mv;
                        mvBi[1] = zero_mv_b;
                    }
                }
            }
            else
                assert( 0 );
        }

        mvCand = mvBi[list];
        if ( mvBi[list].ref == -1 )
        {
            mvCand.ref =  ref_idx;//mvBi[1-list].ref;
            mvCand.x   = -mvBi[1-list].x;
            mvCand.y   = -mvBi[1-list].y;
        }

        assert( mvCand.ref != -1 );
        pInfo->mvCand[ pInfo->iNum++ ] = mvCand;
    }
}

/*!
************************************************************************
* \brief
* Functions for MVP derivation in FastME
*  A: LEFT, B: TOP, C: ABOVE_RIGHT, D: BELOW_LEFT, ...
************************************************************************
*/
int deriveNeighbors( NeighborData* p_Neighbor, int mb_pix_x, int mb_pix_y, NeighborBlock eBlock )
{
    int rvale = 0;
    switch( eBlock )
    {
        case MB_LEFT:
            rvale = deriveLeft( p_Neighbor, mb_pix_x, mb_pix_y );
            break;
        case MB_TOP:
            rvale = deriveTop( p_Neighbor, mb_pix_x, mb_pix_y );
            break;
        case MB_ABOVE_RIGHT:
            rvale = deriveAboveRight( p_Neighbor, mb_pix_x, mb_pix_y );
            break;
        case MB_BELOW_LEFT:
            rvale = deriveBelowLeft( p_Neighbor, mb_pix_x, mb_pix_y );
            break;
        default:
            assert( 0 );
    }
    return rvale;
}

void addMVPCandTempFME( MVPInfo* pInfo, int mb_pix_x, int mb_pix_y, int ref_idx, int mode, int list )
{
    static const int inc_x[6][4] = { {0,0,0,0}, {0,15,0,15}, {-1, 7,8,0},{-1, 0,4,0}, {0,0,0,0}, {0,0,0,0} };
    static const int inc_y[6][4] = { {0,0,0,0}, {0,0,15,15}, { 0,-1,7,8},{ 0,-1,0,4}, {0,0,0,0}, {0,0,0,0} };

    int  pic_pix_x = img->pix_x + mb_pix_x;
    int  pic_pix_y = img->pix_x + mb_pix_y;
    int  idx;
    mv_t mv_cand;

    if ( tr_lastP != 0 )
    {
        for ( idx = 0; idx < 4; idx ++ )
        {
            if ( deriveMVPTemp( &mv_cand, pic_pix_x + inc_x[mode][idx], pic_pix_y + inc_y[mode][idx], ref_idx, list ) )
            {
                pInfo->mvCand[ pInfo->iNum++ ] = mv_cand;
            }
        }
    }
}

int deriveMVPTemp( mv_t *p_mv, int pic_pix_x, int pic_pix_y, int ref_idx, int list )
{
    int prevP_no  =  ( img->number - 1 ) * img->p_interval;
    int nextP_no  =  ( img->number ) * img->p_interval;
    int mb_pix_x  = pic_pix_x % MB_BLOCK_SIZE;
    int mb_pix_y  = pic_pix_y % MB_BLOCK_SIZE;

    int block8x8_addr_x = pic_pix_x >> 3;
    int block8x8_addr_y = pic_pix_y >> 3;
    int block4x4_addr_x = ( mb_pix_x%8 )/4;
    int block4x4_addr_y = ( mb_pix_y%8 )/4;

    const int isign[2] = {1, -1};
    mv_t  mvCand;

    if ( block8x8_addr_y >= 0 && block8x8_addr_y < img->height/8 && block8x8_addr_x >= 0 && block8x8_addr_x < img->width/8 )
    {
        mvCand = last_frm_mv_4x4[ block8x8_addr_y ][ block8x8_addr_x ][ block4x4_addr_y ][ block4x4_addr_x ];
    }
    else
        return 0;

    if ( img->type == B_IMG )
    {
        int distP = nextP_no - prevP_no;
        int distB = ( list == 0 ? img->tr - prevP_no : nextP_no - img->tr );

        if ( mvCand.ref != -1 )
        {
            mvCand.x = isign[list] * ( mvCand.x * distB ) / distP;
            mvCand.y = isign[list] * ( mvCand.x * distB ) / distP;

            if ( list == 0 )
            {
                *p_mv = mvCand;
            }
            else
            {
                *p_mv = mvCand;
                p_mv->ref = ref_idx;
            }
        }
    }
    else // P_IMG
    {
        if ( mvCand.ref != -1 )
        {
            *p_mv = mvCand;
        }
    }

    return ( p_mv->ref == -1 ? 0 : 1 );
}

int  deriveLeft( NeighborData* p_Neighbor, int mb_pix_x, int mb_pix_y )
{
    // LEFT
    int pic_pix_x = mb_pix_x + img->pix_x;
    //  int pic_pix_y = mb_pix_y + img->pix_y;
    int neighbor_mb_pix_x, neighbor_mb_pix_y;
    int neighbor_block_x = -1, neighbor_block_y = -1;
    p_Neighbor->p_MB = NULL;

    if ( mb_pix_x - 1 < 0 ) //outside currentMB
    {
        if ( pic_pix_x - 1 >= 0 && img->mb_data[img->curr_mb_nr - 1].slice_nr == img->mb_data[img->curr_mb_nr].slice_nr ) // MB exists & in the same slice
        {
            if ( img->mb_data[img->curr_mb_nr - 1].mb_type != I8MB && img->mb_data[img->curr_mb_nr - 1].mb_type != I4MB )
            {
                p_Neighbor->p_MB = &img->mb_data[img->curr_mb_nr - 1];
                neighbor_mb_pix_x = MB_BLOCK_SIZE - 1;
                neighbor_mb_pix_y = mb_pix_y;

                neighbor_block_x = ( img->pix_x - 16 ) >> 3;
                neighbor_block_y = img->pix_y >> 3;
            }
        }
    }
    else
    {
        p_Neighbor->p_MB = &img->mb_data[img->curr_mb_nr];
        neighbor_mb_pix_x = mb_pix_x - 1;
        neighbor_mb_pix_y = mb_pix_y;

        neighbor_block_x = img->pix_x >> 3;
        neighbor_block_y = img->pix_y >> 3;
    }

    if ( p_Neighbor->p_MB )
    {
        assert( neighbor_block_x != -1 );
        p_Neighbor->block_x  = ( neighbor_mb_pix_x >> 3 ) + neighbor_block_x;
        p_Neighbor->block_y  = ( neighbor_mb_pix_y >> 3 ) + neighbor_block_y;
        p_Neighbor->block_x4 = ( neighbor_mb_pix_x%8 )/4;
        p_Neighbor->block_y4 = ( neighbor_mb_pix_y%8 )/4;

        p_Neighbor->mb_block_x = neighbor_mb_pix_x >> 3;
        p_Neighbor->mb_block_y = neighbor_mb_pix_y >> 3;

        p_Neighbor->curr_mb_block_x = mb_pix_x >> 3;
        p_Neighbor->curr_mb_block_y = mb_pix_y >> 3;
        return 1;
    }

    return 0;
}

int  deriveTop ( NeighborData* p_Neighbor, int mb_pix_x, int mb_pix_y )
{
    //TOP
    //  int pic_pix_x = mb_pix_x + img->pix_x;
    int pic_pix_y = mb_pix_y + img->pix_y;
    int neighbor_mb_pix_x, neighbor_mb_pix_y;
    int neighbor_block_x = -1, neighbor_block_y = -1;

    p_Neighbor->p_MB = NULL;

    if ( mb_pix_y - 1 < 0 ) //outside currentMB
    {
        if ( pic_pix_y - 1 >= 0 && img->mb_data[ img->curr_mb_nr - img->mb_width ].slice_nr == img->mb_data[img->curr_mb_nr].slice_nr ) // MB exists & in the same slice
        {
            if ( img->mb_data[img->curr_mb_nr - img->mb_width].mb_type != I8MB && img->mb_data[img->curr_mb_nr - img->mb_width].mb_type != I4MB )
            {
                p_Neighbor->p_MB = &img->mb_data[img->curr_mb_nr - img->mb_width];
                neighbor_mb_pix_x = mb_pix_x;
                neighbor_mb_pix_y = MB_BLOCK_SIZE - 1;

                neighbor_block_x = ( img->pix_x ) >> 3;
                neighbor_block_y = ( img->pix_y - 16 ) >> 3;
            }
        }
    }
    else
    {
        p_Neighbor->p_MB = &img->mb_data[img->curr_mb_nr];
        neighbor_mb_pix_x = mb_pix_x;
        neighbor_mb_pix_y = mb_pix_y - 1;

        neighbor_block_x = ( img->pix_x ) >> 3;
        neighbor_block_y = ( img->pix_y ) >> 3;
    }

    if ( p_Neighbor->p_MB )
    {
        assert( neighbor_block_x != -1 );
        p_Neighbor->block_x  = ( neighbor_mb_pix_x >> 3 ) + neighbor_block_x;
        p_Neighbor->block_y  = ( neighbor_mb_pix_y >> 3 ) + neighbor_block_y;
        p_Neighbor->block_x4 = ( neighbor_mb_pix_x%8 )/4;
        p_Neighbor->block_y4 = ( neighbor_mb_pix_y%8 )/4;

        p_Neighbor->mb_block_x = neighbor_mb_pix_x >> 3;
        p_Neighbor->mb_block_y = neighbor_mb_pix_y >> 3;

        p_Neighbor->curr_mb_block_x = mb_pix_x >> 3;
        p_Neighbor->curr_mb_block_y = mb_pix_y >> 3;
        return 1;
    }

    return 0;
}


int  deriveAboveRight( NeighborData* p_Neighbor, int mb_pix_x, int mb_pix_y )
{
    int pic_pix_x = mb_pix_x + img->pix_x;
    int pic_pix_y = mb_pix_y + img->pix_y;
    int neighbor_mb_pix_x, neighbor_mb_pix_y;
    int neighbor_block_x = -1, neighbor_block_y = -1;

    p_Neighbor->p_MB = NULL;

    if ( mb_pix_y - 1 < 0 ) //outside currentMB
    {
        if ( mb_pix_x + 1 < MB_BLOCK_SIZE )
        {
            if ( pic_pix_y - 1 >= 0 && img->mb_data[ img->curr_mb_nr - img->mb_width ].slice_nr == img->mb_data[img->curr_mb_nr].slice_nr ) // MB exists & in the same slice
            {
                if ( img->mb_data[img->curr_mb_nr - img->mb_width].mb_type != I8MB && img->mb_data[img->curr_mb_nr - img->mb_width].mb_type != I4MB )
                {
                    p_Neighbor->p_MB = &img->mb_data[img->curr_mb_nr - img->mb_width];
                    neighbor_mb_pix_x = mb_pix_x + 1;
                    neighbor_mb_pix_y = MB_BLOCK_SIZE - 1;

                    neighbor_block_x = ( img->pix_x ) >> 3;
                    neighbor_block_y = ( img->pix_y - 16 ) >> 3;
                }
            }
        }
        else if ( pic_pix_x + 1 < img->width )
        {
            if ( pic_pix_y - 1 >= 0 && img->mb_data[ img->curr_mb_nr - img->mb_width + 1].slice_nr == img->mb_data[img->curr_mb_nr].slice_nr ) // MB exists & in the same slice
            {
                if ( img->mb_data[img->curr_mb_nr - img->mb_width + 1].mb_type != I8MB && img->mb_data[img->curr_mb_nr - img->mb_width + 1].mb_type != I4MB )
                {
                    p_Neighbor->p_MB = &img->mb_data[img->curr_mb_nr - img->mb_width + 1];
                    neighbor_mb_pix_x = 0;
                    neighbor_mb_pix_y = MB_BLOCK_SIZE - 1;

                    neighbor_block_x = ( img->pix_x + 16 ) >> 3;
                    neighbor_block_y = ( img->pix_y - 16 ) >> 3;
                }
            }
        }
    }
    else
    {
        if ( mb_pix_x + 1 < MB_BLOCK_SIZE )
        {
            p_Neighbor->p_MB = &img->mb_data[img->curr_mb_nr];
            neighbor_mb_pix_x = mb_pix_x + 1;
            neighbor_mb_pix_y = mb_pix_y - 1;

            neighbor_block_x = ( img->pix_x ) >> 3;
            neighbor_block_y = ( img->pix_y ) >> 3;

            if ( ( mb_pix_y % 8 ) && mb_pix_x + 1 == 8 ) // block 3, block 11
                p_Neighbor->p_MB = NULL;
        }
    }

    if ( p_Neighbor->p_MB )
    {
        assert( neighbor_block_x != -1 );
        p_Neighbor->block_x  = ( neighbor_mb_pix_x >> 3 ) + neighbor_block_x;
        p_Neighbor->block_y  = ( neighbor_mb_pix_y >> 3 ) + neighbor_block_y;
        p_Neighbor->block_x4 = ( neighbor_mb_pix_x%8 )/4;
        p_Neighbor->block_y4 = ( neighbor_mb_pix_y%8 )/4;


        p_Neighbor->mb_block_x = neighbor_mb_pix_x >> 3;
        p_Neighbor->mb_block_y = neighbor_mb_pix_y >> 3;

        p_Neighbor->curr_mb_block_x = mb_pix_x >> 3;
        p_Neighbor->curr_mb_block_y = mb_pix_y >> 3;
        return 1;
    }

    return 0;
}

int  deriveBelowLeft ( NeighborData* p_Neighbor, int mb_pix_x, int mb_pix_y )
{
    int pic_pix_x = mb_pix_x + img->pix_x;
    //  int pic_pix_y = mb_pix_y + img->pix_y;
    int neighbor_mb_pix_x, neighbor_mb_pix_y;
    int neighbor_block_x = -1, neighbor_block_y = -1;

    p_Neighbor->p_MB = NULL;

    if ( mb_pix_x - 1 < 0 ) //outside currentMB
    {
        if ( pic_pix_x - 1 >= 0 && mb_pix_y + 1 < MB_BLOCK_SIZE && img->mb_data[img->curr_mb_nr - 1].slice_nr == img->mb_data[img->curr_mb_nr].slice_nr ) // MB exists & in the same slice
        {
            if ( img->mb_data[img->curr_mb_nr - 1].mb_type != I8MB && img->mb_data[img->curr_mb_nr - 1].mb_type != I4MB )
            {
                p_Neighbor->p_MB = &img->mb_data[img->curr_mb_nr - 1];
                neighbor_mb_pix_x = MB_BLOCK_SIZE - 1;
                neighbor_mb_pix_y = mb_pix_y + 1;

                neighbor_block_x = ( img->pix_x - 16 ) >> 3;
                neighbor_block_y = ( img->pix_y ) >> 3;
            }
        }
    }
    else
    {
        if ( mb_pix_x == 8 && ( mb_pix_y % 8 == 3 ) )
        {
            p_Neighbor->p_MB = &img->mb_data[img->curr_mb_nr];
            neighbor_mb_pix_x = mb_pix_x - 1;
            neighbor_mb_pix_y = mb_pix_y + 1;

            neighbor_block_x = ( img->pix_x ) >> 3;
            neighbor_block_y = ( img->pix_y ) >> 3;
        }
    }

    if ( p_Neighbor->p_MB )
    {
        assert( neighbor_block_x != -1 );
        p_Neighbor->block_x  = ( neighbor_mb_pix_x >> 3 ) + neighbor_block_x;
        p_Neighbor->block_y  = ( neighbor_mb_pix_y >> 3 ) + neighbor_block_y;
        p_Neighbor->block_x4 = ( neighbor_mb_pix_x%8 )/4;
        p_Neighbor->block_y4 = ( neighbor_mb_pix_y%8 )/4;


        p_Neighbor->mb_block_x = neighbor_mb_pix_x >> 3;
        p_Neighbor->mb_block_y = neighbor_mb_pix_y >> 3;

        p_Neighbor->curr_mb_block_x = mb_pix_x >> 3;
        p_Neighbor->curr_mb_block_y = mb_pix_y >> 3;
        return 1;
    }


    return 0;
}

/*!
************************************************************************
* \brief
* Functions for SAD prediction of intra block cases.
* 1. void   decide_intrabk_SAD() judges the block coding type(intra/inter)
*    of neighboring blocks
* 2. void skip_intrabk_SAD() set the SAD to zero if neigouring block coding
*    type is intra
* \date
*    2003.4
************************************************************************
*/
void decide_intrabk_SAD()
{
    if ( img->type != I_IMG )
    {
        if ( img->pix_x == 0 && img->pix_y == 0 )
        {
            flag_intra_SAD = 0;
        }
        else if ( img->pix_x == 0 )
        {
            flag_intra_SAD = flag_intra[( img->pix_x )>>4];
        }
        else if ( img->pix_y == 0 )
        {
            flag_intra_SAD = flag_intra[( ( img->pix_x )>>4 )-1];
        }
        else
        {
            flag_intra_SAD = ( ( flag_intra[( img->pix_x )>>4] )||( flag_intra[( ( img->pix_x )>>4 )-1] )||( flag_intra[( ( img->pix_x )>>4 )+1] ) ) ;
        }
    }
    return;
}

void skip_intrabk_SAD( int best_mode, int ref_max )
{
    int i,j,k, ref;
    if ( img->number > 0 )
        flag_intra[( img->pix_x )>>4] = ( best_mode > INTER4x4 ) ? 1:0;

    if ( img->type != I_IMG  && best_mode > INTER4x4 )
    {
        for ( i=0; i < 4; i++ )
        {
            for ( j=0; j < 4; j++ )
            {
                for ( k=0; k < 6; k++ )
                {
                    fastme_l0_cost[k][j][i] = 0;
                    fastme_l1_cost[k][j][i] = 0;
                    for ( ref=0; ref<ref_max; ref++ )
                    {
                        fastme_ref_cost[ref][k][j][i] = 0;
                    }
                }
            }
        }

    }
    return;
}


// block_x, block_y is 8x8 based
void setup_FME( short ref, int list, int block_y, int block_x, int blocktype )
{
    int N_Bframe=0;
    int n_Bframe=0;
    int temp_blocktype = 0;
    int indication_blocktype[6]= {0,0,1,2,1,1};
    N_Bframe = input->succe_b;
    n_Bframe =( N_Bframe ) ? ( tot_b_frm_num % ( N_Bframe+1 ) ): 0;


    /**************************** MV prediction **********************/
    //MV uplayer prediction
    if ( blocktype>1 )
    {
        temp_blocktype = indication_blocktype[blocktype];
        pred_MV_uplayer[0] = img->mv[ref][temp_blocktype][block_y][block_x].x;//all_mv[block_y][block_x][list][ref][temp_blocktype][0];
        pred_MV_uplayer[1] = img->mv[ref][temp_blocktype][block_y][block_x].y;
    }

    //MV ref-frame prediction
    pred_MV_ref_flag = 0;

    if( list==0 )
    {
        if ( ref > 0 )
        {
            pred_MV_ref[0] = img->mv[ref-1][temp_blocktype][block_y][block_x].x;
            pred_MV_ref[0] = ( int )( pred_MV_ref[0]*( ref+1 )/( float )( ref ) );
            pred_MV_ref[1] = img->mv[ref-1][temp_blocktype][block_y][block_x].y;
            pred_MV_ref[1] = ( int )( pred_MV_ref[1]*( ref+1 )/( float )( ref ) );
            pred_MV_ref_flag = 1;
        }

        if ( img->type == B_IMG ) //B frame forward prediction, first ref
        {
            pred_MV_ref[0] =( int ) ( img->mv[1-list][blocktype][block_y][block_x].x*( -n_Bframe )/( N_Bframe-n_Bframe+1.0f ) ); //(all_mv[block_y][block_x][1][0][blocktype][0]
            pred_MV_ref[1] =( int ) ( img->mv[1-list][blocktype][block_y][block_x].y*( -n_Bframe )/( N_Bframe-n_Bframe+1.0f ) ); //(all_mv[block_y][block_x][1][0][blocktype][1]
            pred_MV_ref_flag = 1;
        }

    }
    /******************************SAD prediction**********************************/
    if ( list==0 && ref>0 ) //pred_SAD_ref
    {

        if ( flag_intra_SAD ) //add this for irregular motion
        {
            pred_SAD = 0;
        }
        else
        {
            pred_SAD = fastme_ref_cost[ref-1][blocktype][block_y][block_x];
        }
    }
    else if ( blocktype>1 ) // pred_SAD_uplayer
    {
        if ( flag_intra_SAD )
        {
            pred_SAD = 0;
        }
        else
        {
            pred_SAD = ( list==1 ) ? ( fastme_l1_cost[temp_blocktype][( img->pix_y>>3 )+block_y][( img->pix_x>>3 )+block_x] ) : ( fastme_l0_cost[temp_blocktype][( img->pix_y>>3 )+block_y][( img->pix_x>>3 )+block_x] );

            pred_SAD /= 4;
        }
    }
    else pred_SAD = 0 ;  // pred_SAD_space
}
